package Physics;

public class Matrix3{
	
	public float[] data = new float[9];
	// constructor
	public Matrix3(){
		
			data[0] = 
			data[1] =
			data[2] =
			data[3] =
			data[4] =
			data[5] =
			data[6] =
			data[7] =
			data[8] = 0;
	}	
	// creates a new matrix from the three given vectors
	public Matrix3(Vector3 compOne, Vector3 compTwo,
        Vector3 compThree){
    	
        setComponents(compOne, compTwo, compThree);
    }
    
    // create a new matrix with explicit values
	public Matrix3(float c0, float c1, float c2, float c3, float c4, 
    		float c5, float c6, float c7, float c8){
    	
        data[0] = c0; data[1] = c1; data[2] = c2;
        data[3] = c3; data[4] = c4; data[5] = c5;
        data[6] = c6; data[7] = c7; data[8] = c8;
    }    
	
	public Matrix3(Matrix3 other)
	{
		for (int i = 0; i < 9; ++i)
			data[i] = other.data[i];
	}
    
    // create a diagonal matrix using the 3 given values
	public void setDiagonal(float a, float b, float c)
    {
        setInertiaTensorCoeffs(a, b, c);
    }    
    
    // creates an intertia tensor based matrix
	public void setInertiaTensorCoeffs(float ix, float iy, float iz)
    {
    	float ixy=0; float ixz=0; float iyz=0;
    	
        data[0] = ix;
        data[1] = data[3] = -ixy;
        data[2] = data[6] = -ixz;
        data[4] = iy;
        data[5] = data[7] = -iyz;
        data[8] = iz;
    }
    // creates the inertia tensor of a rectangular block
	public void setBlockInertiaTensor(Vector3 halfSizes, float mass)
    {
        Vector3 squares = halfSizes.componentProduct(halfSizes);
        setInertiaTensorCoeffs(0.3f*mass*(squares.y + squares.z),
            0.3f*mass*(squares.x + squares.z),
            0.3f*mass*(squares.x + squares.y));
    } 
    
    // creates a skew matrix
	public void setSkewSymmetric(Vector3 vector)
    {
        data[0] = data[4] = data[8] = 0;
        data[1] = -vector.z;
        data[2] = vector.y;
        data[3] = vector.z;
        data[5] = -vector.x;
        data[6] = -vector.y;
        data[7] = vector.x;
    }
    
    // sets the components of a matrix from the given vectors
	public void setComponents(Vector3 compOne, Vector3 compTwo,
        Vector3 compThree)
    {
        data[0] = compOne.x;
        data[1] = compTwo.x;
        data[2] = compThree.x;
        data[3] = compOne.y;
        data[4] = compTwo.y;
        data[5] = compThree.y;
        data[6] = compOne.z;
        data[7] = compTwo.z;
        data[8] = compThree.z;

    }   
   
    // multiply a vector by a matrix to get an output vector
	public Vector3 times(Vector3 vector) 
    {
        return new Vector3(
            vector.x * data[0] + vector.y * data[1] + vector.z * data[2],
            vector.x * data[3] + vector.y * data[4] + vector.z * data[5],
            vector.x * data[6] + vector.y * data[7] + vector.z * data[8]
        );
    }

	// another way to multiply
	public Vector3 transform(Vector3 vector) 
    {
    	
        return this.times(vector);
    }

    // transform the vector by the matrix transpose
	public Vector3 transformTranspose(Vector3 vector) 
    {
        return new Vector3(
            vector.x * data[0] + vector.y * data[3] + vector.z * data[6],
            vector.x * data[1] + vector.y * data[4] + vector.z * data[7],
            vector.x * data[2] + vector.y * data[5] + vector.z * data[8]
        );
    }

    // get a vector from the given row id
	public Vector3 getRowVector(int i) 
    {
        return new Vector3(data[i*3], data[i*3+1], data[i*3+2]);
    }

    // get a vector from the given axis id
    Vector3 getAxisVector(int i) 
    {
        return new Vector3(data[i], data[i+3], data[i+6]);
    }    
    // sets the matrix to the inverse of the given matrix
    public void setInverse(Matrix3 m)
    {
        float t4 = m.data[0]*m.data[4];
        float t6 = m.data[0]*m.data[5];
        float t8 = m.data[1]*m.data[3];
        float t10 = m.data[2]*m.data[3];
        float t12 = m.data[1]*m.data[6];
        float t14 = m.data[2]*m.data[6];

        // Calculate the determinant
        float t16 = (t4*m.data[8] - t6*m.data[7] - t8*m.data[8]+
                    t10*m.data[7] + t12*m.data[5] - t14*m.data[4]);

        // Make sure the determinant is non-zero.
        if (t16 == (float)0.0f) return;
        float t17 = 1/t16;

        data[0] = (m.data[4]*m.data[8]-m.data[5]*m.data[7])*t17;
        data[1] = -(m.data[1]*m.data[8]-m.data[2]*m.data[7])*t17;
        data[2] = (m.data[1]*m.data[5]-m.data[2]*m.data[4])*t17;
        data[3] = -(m.data[3]*m.data[8]-m.data[5]*m.data[6])*t17;
        data[4] = (m.data[0]*m.data[8]-t14)*t17;
        data[5] = -(t6-t10)*t17;
        data[6] = (m.data[3]*m.data[7]-m.data[4]*m.data[6])*t17;
        data[7] = -(m.data[0]*m.data[7]-t12)*t17;
        data[8] = (t4-t8)*t17;
    }

    // returns the inverse of this matrix
    public Matrix3 inverse() 
    {
        Matrix3 result = new Matrix3();
        result.setInverse(this);
        return result;
    }

    // inverts this matrix
    public void invert()
    {
        setInverse(this);
    }    
    // sets the given matrix to the tranpose of this matrix
    public void setTranspose(Matrix3 a_m)
    {
        data[0] = a_m.data[0];
        data[1] = a_m.data[3];
        data[2] = a_m.data[6];
        data[3] = a_m.data[1];
        data[4] = a_m.data[4];
        data[5] = a_m.data[7];
        data[6] = a_m.data[2];
        data[7] = a_m.data[5];
        data[8] = a_m.data[8];
    }

    // returns the transpose of this matrix
    public Matrix3 transpose() 
    {
        Matrix3 result = new Matrix3();
        result.setTranspose(this);
        return result;
    }

    // returns a matrix multiple
    // overloaded operator* has been changed to Times
    public Matrix3 multiply(Matrix3 a_o) 
    {
        return new Matrix3(
            data[0]*a_o.data[0] + data[1]*a_o.data[3] + data[2]*a_o.data[6],
            data[0]*a_o.data[1] + data[1]*a_o.data[4] + data[2]*a_o.data[7],
            data[0]*a_o.data[2] + data[1]*a_o.data[5] + data[2]*a_o.data[8],

            data[3]*a_o.data[0] + data[4]*a_o.data[3] + data[5]*a_o.data[6],
            data[3]*a_o.data[1] + data[4]*a_o.data[4] + data[5]*a_o.data[7],
            data[3]*a_o.data[2] + data[4]*a_o.data[5] + data[5]*a_o.data[8],

            data[6]*a_o.data[0] + data[7]*a_o.data[3] + data[8]*a_o.data[6],
            data[6]*a_o.data[1] + data[7]*a_o.data[4] + data[8]*a_o.data[7],
            data[6]*a_o.data[2] + data[7]*a_o.data[5] + data[8]*a_o.data[8]
            );
    }

    // matrix multiple and assignment
    // overloaded operator*= has been changed to Times Equals
    public void multiplyAndSet(Matrix3 a_o1)
    {
        float t1;
        float t2;
        float t3;

        t1 = data[0]*a_o1.data[0] + data[1]*a_o1.data[3] + data[2]*a_o1.data[6];
        t2 = data[0]*a_o1.data[1] + data[1]*a_o1.data[4] + data[2]*a_o1.data[7];
        t3 = data[0]*a_o1.data[2] + data[1]*a_o1.data[5] + data[2]*a_o1.data[8];
        data[0] = t1;
        data[1] = t2;
        data[2] = t3;

        t1 = data[3]*a_o1.data[0] + data[4]*a_o1.data[3] + data[5]*a_o1.data[6];
        t2 = data[3]*a_o1.data[1] + data[4]*a_o1.data[4] + data[5]*a_o1.data[7];
        t3 = data[3]*a_o1.data[2] + data[4]*a_o1.data[5] + data[5]*a_o1.data[8];
        data[3] = t1;
        data[4] = t2;
        data[5] = t3;

        t1 = data[6]*a_o1.data[0] + data[7]*a_o1.data[3] + data[8]*a_o1.data[6];
        t2 = data[6]*a_o1.data[1] + data[7]*a_o1.data[4] + data[8]*a_o1.data[7];
        t3 = data[6]*a_o1.data[2] + data[7]*a_o1.data[5] + data[8]*a_o1.data[8];
        data[6] = t1;
        data[7] = t2;
        data[8] = t3;
    }

    // multiplies the matrix by the scalar
    // operator *= has been changed to TimesEquals
    public void multiplyAndSet(float scalar)
    {
        data[0] *= scalar; data[1] *= scalar; data[2] *= scalar;
        data[3] *= scalar; data[4] *= scalar; data[5] *= scalar;
        data[6] *= scalar; data[7] *= scalar; data[8] *= scalar;
    }

    // component-wise addition of two matrices
    // overloaded operator+= has been changed to PlusEquals
    public void addAndSet(Matrix3 a_oo)
    {
        data[0] += a_oo.data[0]; data[1] += a_oo.data[1]; data[2] += a_oo.data[2];
        data[3] += a_oo.data[3]; data[4] += a_oo.data[4]; data[5] += a_oo.data[5];
        data[6] += a_oo.data[6]; data[7] += a_oo.data[7]; data[8] += a_oo.data[8];
    }

    // sets the matrix to be a rotation matrix of
	// the given Quarternion
    public void setOrientation(Quaternion q)
    {
        data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
        data[1] = 2*q.i*q.j + 2*q.k*q.r;
        data[2] = 2*q.i*q.k - 2*q.j*q.r;
        data[3] = 2*q.i*q.j - 2*q.k*q.r;
        data[4] = 1 - (2*q.i*q.i  + 2*q.k*q.k);
        data[5] = 2*q.j*q.k + 2*q.i*q.r;
        data[6] = 2*q.i*q.k + 2*q.j*q.r;
        data[7] = 2*q.j*q.k - 2*q.i*q.r;
        data[8] = 1 - (2*q.i*q.i  + 2*q.j*q.j);
    }

    /* TODO--------------------------------------->
	D3DXMATRIX ConvertToDX()
	{
		D3DXMATRIX w;

		w._11 = (float)data[0];
		w._12 = (float)data[3];
		w._13 = (float)data[6];
		w._14 = 0;

		w._21 = (float)data[1];
		w._22 = (float)data[4];
		w._23 = (float)data[7];
		w._24 = 0;

		w._31 = (float)data[2];
		w._32 = (float)data[5];
		w._33 = (float)data[8];
		w._34 = 0;

		w._41 = 0;
		w._42 = 0;
		w._43 = 0;
		w._44 = 1;

		return w;
	}
	*/
    
    // interpolates a couple of matrices
    public static Matrix3 linearInterpolate(Matrix3 a_m, Matrix3 a_b, double prop)
    {
        Matrix3 result = new Matrix3();
        for (int i = 0; i < 9; i++) {
            result.data[i] = (float) (a_m.data[i] * (1-prop) + a_b.data[i] * prop);
        }
        return result;
    }
}